'''
Created on Dec 8, 2013

@author: dross
'''
from nxt.locator import Method
import nxt
from nxt.motor import Motor, PORT_C, PORT_B, PORT_A
from nxt.sensor.generic import Ultrasonic, Color20
from nxt.sensor.common import PORT_4, PORT_3, Type
import thread

class Rover(object):
    '''
    Simple Rover Robot Class
    '''


    def __init__(self):
        '''
        Constructor
        '''
        
    FREQ_C = 523
    FREQ_D = 587
    FREQ_E = 659
    FREQ_G = 784

    def connect(self):
        m = Method(usb=False, bluetooth=True)
        self.brick = nxt.locator.find_one_brick(host="00:16:53:17:EC:0A", debug=True, method=m)
        b = self.brick
        self.wheel_R = Motor(b, PORT_C)
        self.wheel_L = Motor(b, PORT_B)
        self.arm = Motor(b, PORT_A)
        self.ultraSensor = Ultrasonic(b, PORT_4,)
        self.colorSensor = Color20(b, PORT_3)
     
    def beep(self):
        self.brick.play_tone_and_wait(self.FREQ_E, 500)   
        
    def shutdown(self):
        self.wheel_L.idle()  
        self.wheel_R.idle()
        self.arm.idle()
        self.colorSensor.set_light_color(Type.COLORNONE)
    
    def turnmotor(self, m, power, degrees):
        m.turn(power, degrees)
        
    def moveForward(self, rotations=1, speed=75):
        degrees = 360 * rotations
        thread.start_new_thread(
                                self.turnmotor,
                                (self.wheel_L, speed, degrees))
        thread.start_new_thread(
                                self.turnmotor,
                                (self.wheel_R, speed, degrees))
 
    def moveBackward(self, rotations=1, speed=75):
        self.moveForward(rotations, -speed)
   
        